﻿using DLLPC.Utilities;
using HalconDotNet;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace VisionCore
{
    public class CoordinateHelper
    {
        public static HHomMat2D HomMat2D_1 = new HHomMat2D();

        public static bool CalcHomMat2D_Offset(MarkInfo[] marks)
        {
            HTuple htWorldX = new HTuple();
            HTuple htWorldY = new HTuple();
            HTuple htCCDX = new HTuple();
            HTuple htCCDY = new HTuple();

            //旋转60度，获取中垂线上的点
            marks[2].Mark = PointRotation.RotatePoint(marks[0].Mark, marks[1].Mark, Math.PI / 3);
            marks[2].PMark = PointRotation.RotatePoint(marks[0].PMark, marks[1].PMark, Math.PI / 3);

            for (int i = 0; i < 3; i++)
            {
                htCCDX.Append(marks[i].Mark.x);
                htCCDY.Append(marks[i].Mark.y);
                htWorldX.Append(marks[i].PMark.x);
                htWorldY.Append(marks[i].PMark.y);
            }

            HomMat2D_1.VectorToHomMat2d(htCCDX, htCCDY, htWorldX, htWorldY);

           
            double d1, d2, d3, d4, d5;
            double d0 = HomMat2D_1.HomMat2dToAffinePar(out d1, out d2, out d3, out d4, out d5);
            string strHomMat = string.Format("对位补正计算: Sx缩放{0:F4},Sy缩放{1:F4},Phi旋转{2:F4},Tx平移{3:F4},Ty平移{4:F4}", d0, d1, d2, d4, d5);
            LogMsg(strHomMat);

            return (Math.Abs(d0 - 1) < 0.1) && (Math.Abs(d1 - 1) < 0.1) && (Math.Abs(d2 + 0) < 5);
        }

        public static PointRotation.Point GetTrayPos(double ccdX, double ccdY)
        {
            PointRotation.Point p = new PointRotation.Point();
            double d1X;
            double d1Y;

            d1X = HomMat2D_1.AffineTransPoint2d(ccdX, ccdY, out d1Y);

            //HHomMat2D hHomMat2D = new HHomMat2D();
            //hHomMat2D.HomMat2dTranslate(1,1.0);
            //d1X = HomMat2D_1.AffineTransPoint2d(d1X, d1Y, out d1Y);

            p.x = d1X;
            p.y = d1Y;

            return p;
        }

        private static void LogMsg(string Msg)
        {
            LogHelper.Write(LogHelper.Module.VisionProcess, Msg);
        }
    }
}
